function [resid,jac,systemData,Fh_torque,totalRotation] = ...
    basicBoot(pivotRotations,Fh,systemData)
% [resid,jac,systemData,Fh_torque] =
% basicBoot(pivotRotations,Fh,systemData)
% Function to return boot/binding reaction for given conditions
% inputs: systemData -> structure of boot and binding data, using the
% following
%             *.stuff
%             *.boot.soleLength - total sole length of boot in meters
%             *.
%         pivotRotations = [alpha-toe, alpha-bellows]
%         Fh = heel lift force
% outputs
% residual = sum of moments about [toe, bellows]
% jacobian = jacobian of residual wrt pivot rotations
% systemData = new data struc with any additional terms added in this func
% Fh_torque = torque produced by heel force in given configuration


forefootratio = 0.346; % fraction of forefoot (foreward of bellows)
% length to total boot sole length

bellowsStiffness = 60; %torsional spring stiffness of the bellows
bellowsLimitStiffness = 1e4; %addative torsional spring stiffness of bellows once at limit
bellowsAngleLimit = 20*pi/180; %limit angle of bellows
toeLimitStiffness = 1e2; %torsional spring stiffness of toe to prevent rotation downward
toeBendStiffness = 80; %torsional spring stiffness of toe resisting rotation - simulate clamping of toe
toeSlopAngle = 5*pi/180;


alphaB = pivotRotations(2); % angle from ref for bellows
alphaT = pivotRotations(1); % angle from ref for toe of boot

Lt = systemData.boot.soleLength; %total sole length
Lf = Lt*forefootratio; %forefoot boot sole length (forward of bellows)
Lr = Lt*(1-forefootratio); % length from bellows rearward

bootSoleOffset = 1e-2; % offset from pivot point and heel pivot location to 
% bottom of sole, currently assuming pivot around toebox bar, so top of
% duckbill where meets toe of boot, using this as origin for x and y
% positions

% for derivatives alpha vector is (toe,bellows)

xbellows = [Lf*cos(alphaT);Lf*sin(alphaT)];

xheel = xbellows+[Lr*cos(alphaT+alphaB); Lr*sin(alphaT+alphaB)];

Rth = xheel; %vector from toe pivot to heel

Rtb = xbellows; %vector from toe pivot to bellows pivot

Rbt = -Rtb; %vector from bellows to toe, just opposite of toe to bellows

Rbh = xheel - xbellows; %vector from bellows pivot to heel

vT = [-Rth(2);Rth(1)]; 

hT = vT./norm(vT); %get orientation of heel force (force of person pulling on heel)
% and normalize

initialXheel = [Lt,0]'; %initial heel position
systemData.boot.initialXheel = initialXheel;

Fb = bindingModels(xheel,systemData);

MtL = (alphaT < 0)*abs(alphaT)*toeLimitStiffness + ...
    -(alphaT > (0+toeSlopAngle))*(alphaT - toeSlopAngle)*toeBendStiffness;  %moment from toe, currently being
% used as a constraint to keep toe from pivoting down, should be large
% stiffness, might need further work or simply a better inequality constraint approach


%moment from bellows, resultant from hitting limit angle (first part) and
%from general stiffness (second part)
MbL = (alphaB > bellowsAngleLimit)*(alphaB - bellowsAngleLimit)*bellowsLimitStiffness + ...
    alphaB*bellowsStiffness+ ...
    (alphaB < 0)*(alphaB)*bellowsLimitStiffness;
MbL = -MbL;

% -------------------------- calc moments which are residuals ----
%%% to satisfy static equilibrium, we want moments equal to zero, so thats
%%% what we will be trying to do, given a heel lift force Fh, we are tryin
%%% gto find the corresponding angles of bellows and toe to satisfy the
%%% equilibrium
%%% We want to also output the derivatives of our moment equations with
%%% respect to the angles so that we can use some newton type method to
%%% solve the equations nicely

% first resid/moment is the sum of moments about the toe
% contributions are the toe moment (resulting from toe hitting lower limit)
% the heel lift force, and the binding force
%sigmaMt = Mti + cross(Rth,Fb) + cross(Rth,hT*Fh);
sigmaMt = MtL + (Rth(1)*Fb(2)-Rth(2)*Fb(1)) + (Rth(1)*Fh*hT(2) - Rth(2)*Fh*hT(1)); 


Fb_torque = (Rth(1)*Fb(2)-Rth(2)*Fb(1));

Fh_torque = (Rth(1)*Fh*hT(2) - Rth(2)*Fh*hT(1));

% get force at pin joint at toe
Ft = -(Fb + Fh.*hT);

% second residual/moment is sum of moments about the bellows
sigmaMb = (Rbt(1)*Ft(2) - Rbt(2)*Ft(1)) + ...
    MbL + ...
    (Rbh(1)*Fb(2) - Rbh(2)*Fb(1)) + ...
    Fh*(Rbh(1)*hT(2) - Rbh(2)*hT(1));

    
% --------------- setup outputs ------------------------
resid = [sigmaMt;sigmaMb];
% jac = [dMT_da;dMB_da];
jac = zeros(2,2);
totalRotation = atan(xheel(2)/xheel(1));

